function particles = initialize_mcl_particles(N, mean_pose, std_pose)
% 初始化蒙特卡洛定位粒子
% 输入: N-粒子数, mean_pose-初始位姿均值, std_pose-标准差
% 输出: particles-粒子数组

    particles = struct('pose', {}, 'weight', {});
    
    for i = 1:N
        particles(i).pose = [
            mean_pose(1) + randn() * std_pose(1);
            mean_pose(2) + randn() * std_pose(2);
            wrapToPi(mean_pose(3) + randn() * std_pose(3))
        ];
        particles(i).weight = 1/N;
    end
end

